/*
 * sfd_header.c
 * Created on July 20, 2005; 8:11 PM
 * by VRamadurai
 */
#include <stdio.h>
#include <stdlib.h>

/************ Private Header Files **************/
#include "HsTypes.h"
#include "PhyDebug.h"
#include "PhyStruct.h"
#include "PmdGlobals.h"
#include "PhyConstants.h"
#include "iqdma_3500.h"
#include "rx_functions.h"
#include "rx1_functions.h"
#include "sfd_header.h"
#include "equalizer.h"
#include "thread_globals.h"
#include "threads_common.h"//add 627

/*
static inline
hs_int32_t check_sfd_pattern(hs_uint16_t usSFDPat)
{
    hs_int32_t sfd;
    sfd = ((usSFDPat&0xffff) == 0x5cf);
    return(sfd);
}
*/
void
equalizer_pre(
                struct HS_Phy11bStr  *HS_Phy11bPtrTh,
                hs_int16_t *ssSineCosineTable
              )
{
    hs_int16_t ssInI[32+32];
    hs_int16_t ssInQ[32+32];
    hs_int16_t cos_fixed[32];
    hs_int16_t sin_fixed[32];
    hs_int16_t tempI[32];
    hs_int16_t tempQ[32];

    hs_int16_t *ssOutI;
    hs_int16_t *outIBase;
    hs_int16_t *sincos_t;

    hs_int32_t outbuff_toggle;
    hs_uint32_t uiRotorTableIndex;
    hs_int16_t cos_val;
    hs_int16_t sin_val;
    //hs_int32_t i;

    hs_int32_t siPhaseUpdate = HS_Phy11bPtrTh->siPhaseUpdate;

    sincos_t = HS_Phy11bPtrTh->sincos_t;

    /** Start of equalizer training **/
    /** Equalize the first N_PRE samples to accomodate the delay **/
    sync_on_dma((hs_int32_t)HS_Phy11bPtrTh->piRdPtrI, 2*N_PRE*2);

    START_CLOCK

    rx_copysamps14_update(HS_Phy11bPtrTh, ssInI, ssInQ);

    outIBase = ssOutIQ_shared;
    outbuff_toggle = HS_Phy11bPtrTh->outbuff_toggle_th1;
    ssOutI = outIBase + outbuff_toggle*OUTBUFFSIZE*2;
    HS_Phy11bPtrTh->outbuff_toggle_th1 ^= 1;

    uiRotorTableIndex =(((HS_Phy11bPtrTh->siRotorTableCounter+ 0x200000)&0x7fffffff) >> 22);
    cos_val = ssSineCosineTable[uiRotorTableIndex];
    sin_val = ssSineCosineTable[513+uiRotorTableIndex];

    __sb_rbroad(cos_fixed, (cos_val<<16), 0);
    __sb_rbroad(sin_fixed, (sin_val<<16), 0);
    __sb_rshf1(tempI, tempI+16, ssInI, ssInI+16, 7);
    __sb_rshf1(tempQ, tempQ+16, ssInQ, ssInQ+16, 7);

    __sb_rmuls(ssInI, tempI, sincos_t);
    __sb_rmacs(ssInI, tempQ, sincos_t+32, ssInI);
    __sb_rmuls(ssInQ, tempI, sincos_t+32);
    __sb_rmsubs(ssInQ, tempQ, sincos_t, ssInQ);

    __sb_rmuls(tempI, ssInI, cos_fixed);
    __sb_rmacs(ssOutI, ssInQ, sin_fixed, tempI);
    __sb_rmuls(tempQ, ssInI, sin_fixed);
    __sb_rmsubs(ssOutI+OUTBUFFSIZE, ssInQ, cos_fixed, tempQ);

    HS_Phy11bPtrTh->siRotorTableCounter += (2*siPhaseUpdate*N_PRE);

    /** sync with thread2 **/
    while(thread1to2_flag1 == 1){
        __sb_barrier();
    }
    thread1to2_flag1 = 1;

    PRINT_CYCLES("eq_pre 1");
}


void
wlan_sfd(
                struct HS_Phy11bStr  *HS_Phy11bPtrTh,
                ePHYSTATES_TH *ePhyState,
                hs_int16_t *ssSineCosineTable
             )
{
	#define NUM_SFD_ITER	3

    hs_int32_t i;
    hs_int16_t ssInI[96+32];
    hs_int16_t ssInQ[96+32];
    hs_int16_t cos_fixed[32];
    hs_int16_t sin_fixed[32];
    hs_int16_t tempI[96];
    hs_int16_t tempQ[96];

    hs_int16_t *ssOutI;
    hs_int16_t *outIBase;
    hs_int16_t *sincos_t;

    hs_int32_t outbuff_toggle;
    hs_uint32_t uiRotorTableIndex;
    hs_int32_t siRotorTableCounter;
    hs_int16_t cos_val;
    hs_int16_t sin_val;
    hs_int32_t rem_count;

    hs_int32_t eq_in_index;

    hs_int32_t siPhaseUpdate = HS_Phy11bPtrTh->siPhaseUpdate;
    volatile hs_int32_t *thread1to2_flag1_ptr = &thread1to2_flag1;

    sincos_t = HS_Phy11bPtrTh->sincos_t;

    outIBase = ssOutIQ_shared;
    outbuff_toggle = HS_Phy11bPtrTh->outbuff_toggle_th1;
    siRotorTableCounter = HS_Phy11bPtrTh->siRotorTableCounter;

    /** Wait for thread3 to update the eq index **/
    while(thread3_eq_index_flag == 0){
        __sb_barrier();
    }

    eq_in_index = HS_Phy11bPtrTh->eq_in_index + N_PRE;
    rem_count = PREAMBLE_DOWNSAMPLE_REFSIZE - eq_in_index;

    while(1)
    {
        if(rem_count >= 44){

        	sync_on_dma((hs_int32_t)HS_Phy11bPtrTh->piRdPtrI, 176);

        	START_CLOCK

        	rx_copysamps88_update(HS_Phy11bPtrTh, ssInI, ssInQ);

	        ssOutI = outIBase + outbuff_toggle*OUTBUFFSIZE*2;
	        outbuff_toggle ^= 1;

        	for(i=0; i<NUM_SFD_ITER; i++){
	            __sb_rshf1(tempI+i*16, tempI+i*16+48, ssInI+i*32, ssInI+i*32+16, 7);
	            __sb_rshf1(tempQ+i*16, tempQ+i*16+48, ssInQ+i*32, ssInQ+i*32+16, 7);
	        }

        	for(i=0; i<NUM_SFD_ITER; i++){
        		uiRotorTableIndex =(((siRotorTableCounter+ 0x200000)&0x7fffffff) >> 22);
		        cos_val = ssSineCosineTable[uiRotorTableIndex];
		        sin_val = ssSineCosineTable[513+uiRotorTableIndex];

		        __sb_rbroad(cos_fixed, (cos_val<<16), 0);
		        __sb_rbroad(sin_fixed, (sin_val<<16), 0);

		        __sb_rmuls(ssInI, tempI+i*16, sincos_t);
		        __sb_rmacs(ssInI, tempQ+i*16, sincos_t+32, ssInI);
		        __sb_rmuls(ssInQ, tempI+i*16, sincos_t+32);
		        __sb_rmsubs(ssInQ, tempQ+i*16, sincos_t, ssInQ);

		        __sb_rmuls(tempI, ssInI, cos_fixed);
		        __sb_rmacs(ssOutI+i*16, ssInQ, sin_fixed, tempI);
		        __sb_rmuls(tempQ, ssInI, sin_fixed);
		        __sb_rmsubs(ssOutI+OUTBUFFSIZE+i*16, ssInQ, cos_fixed, tempQ);

		        siRotorTableCounter += (2*siPhaseUpdate*16);
        	}

        	siRotorTableCounter -= (2*siPhaseUpdate*4);
        	rem_count -= 44;
        	eq_in_index += (4*11);
        }
        else{

        	sync_on_dma((hs_int32_t)HS_Phy11bPtrTh->piRdPtrI, 44);

        	START_CLOCK

        	rx_copysamps22_update(HS_Phy11bPtrTh, ssInI, ssInQ);

	        ssOutI = outIBase + outbuff_toggle*OUTBUFFSIZE*2;
	        outbuff_toggle ^= 1;

        	uiRotorTableIndex =(((siRotorTableCounter+ 0x200000)&0x7fffffff) >> 22);
	        cos_val = ssSineCosineTable[uiRotorTableIndex];
	        sin_val = ssSineCosineTable[513+uiRotorTableIndex];

	        __sb_rshf1(tempI, tempI+16, ssInI, ssInI+16, 7);
	        __sb_rshf1(tempQ, tempQ+16, ssInQ, ssInQ+16, 7);
	        __sb_rbroad(cos_fixed, (cos_val<<16), 0);
	        __sb_rbroad(sin_fixed, (sin_val<<16), 0);

	        __sb_rmuls(ssInI, tempI, sincos_t);
	        __sb_rmacs(ssInI, tempQ, sincos_t+32, ssInI);
	        __sb_rmuls(ssInQ, tempI, sincos_t+32);
	        __sb_rmsubs(ssInQ, tempQ, sincos_t, ssInQ);

	        __sb_rmuls(tempI, ssInI, cos_fixed);
	        __sb_rmacs(ssOutI, ssInQ, sin_fixed, tempI);
	        __sb_rmuls(tempQ, ssInI, sin_fixed);
	        __sb_rmsubs(ssOutI+OUTBUFFSIZE, ssInQ, cos_fixed, tempQ);

	        siRotorTableCounter += (2*siPhaseUpdate*11);
	        rem_count -= 11;
	        eq_in_index += (11);
        }


        /** sync with thread2 **/
        while(*thread1to2_flag1_ptr == 1){
            __sb_barrier();
        }

        *thread1to2_flag1_ptr = 1;

        if(eq_in_index >= PREAMBLE_DOWNSAMPLE_REFSIZE){
            *ePhyState = PHY_RX_STATE_HEADER_TH;
            break;
        }
    }

    PRINT_CYCLES("sfd detection 1");

    HS_Phy11bPtrTh->siRotorTableCounter = siRotorTableCounter;
    HS_Phy11bPtrTh->outbuff_toggle_th1 = outbuff_toggle;
}


void
wlan_do_header(
                struct HS_Phy11bStr  *HS_Phy11bPtrTh,
                hs_int16_t *ssSineCosineTable
             )
{
    #define NUM_HEAD_ITER    3

    hs_int32_t i;
    hs_int16_t ssInI[96+32];
    hs_int16_t ssInQ[96+32];
    hs_int16_t cos_fixed[32];
    hs_int16_t sin_fixed[32];
    hs_int16_t tempI[96];
    hs_int16_t tempQ[96];

    hs_int16_t *ssOutI;
    hs_int16_t *outIBase;
    hs_int16_t *sincos_t;

    hs_int32_t outbuff_toggle;
    hs_uint32_t uiRotorTableIndex;
    hs_int32_t siRotorTableCounter;
    hs_int16_t cos_val;
    hs_int16_t sin_val;

    hs_int32_t symbol_count;
    hs_int32_t count;

    hs_int32_t siPhaseUpdate = HS_Phy11bPtrTh->siPhaseUpdate;
    volatile hs_int32_t *thread1to2_flag1_ptr = &thread1to2_flag1;

    sincos_t = HS_Phy11bPtrTh->sincos_t;

    outIBase = ssOutIQ_shared;
    outbuff_toggle = HS_Phy11bPtrTh->outbuff_toggle_th1;
    siRotorTableCounter = HS_Phy11bPtrTh->siRotorTableCounter;

    symbol_count = 0;

    while(1)
    {
        sync_on_dma((hs_int32_t)HS_Phy11bPtrTh->piRdPtrI, 176);

        START_CLOCK

        rx_copysamps88_update(HS_Phy11bPtrTh, ssInI, ssInQ);

        for(i=0; i<NUM_HEAD_ITER; i++){
            __sb_rshf1(tempI+i*16, tempI+i*16+48, ssInI+i*32, ssInI+i*32+16, 7);
            __sb_rshf1(tempQ+i*16, tempQ+i*16+48, ssInQ+i*32, ssInQ+i*32+16, 7);
        }

        ssOutI = outIBase + outbuff_toggle*OUTBUFFSIZE*2;
        outbuff_toggle ^= 1;

        for(count=0; count<NUM_HEAD_ITER; count++)
        {
            uiRotorTableIndex =(((siRotorTableCounter+ 0x200000)&0x7fffffff) >> 22);
            cos_val = ssSineCosineTable[uiRotorTableIndex];
            sin_val = ssSineCosineTable[513+uiRotorTableIndex];

            __sb_rbroad(cos_fixed, (cos_val<<16), 0);
            __sb_rbroad(sin_fixed, (sin_val<<16), 0);

            __sb_rmuls(ssInI, tempI+count*16, sincos_t);
            __sb_rmacs(ssInI, tempQ+count*16, sincos_t+32, ssInI);
            __sb_rmuls(ssInQ, tempI+count*16, sincos_t+32);
            __sb_rmsubs(ssInQ, tempQ+count*16, sincos_t, ssInQ);

            __sb_rmuls(tempI, ssInI, cos_fixed);
            __sb_rmacs(ssOutI+count*16, ssInQ, sin_fixed, tempI);
            __sb_rmuls(tempQ, ssInI, sin_fixed);
            __sb_rmsubs(ssOutI+OUTBUFFSIZE+count*16, ssInQ, cos_fixed, tempQ);

            siRotorTableCounter += (2*siPhaseUpdate*16);
        }

        siRotorTableCounter -= (2*siPhaseUpdate*4);

        /** sync with thread2 **/
        while(*thread1to2_flag1_ptr == 1){
            __sb_barrier();
        }

        *thread1to2_flag1_ptr = 1;

        symbol_count+=4;

        if(symbol_count == N_HEADER_SYMB){
            break;
        }
    }

    PRINT_CYCLES("header detection 1");

    HS_Phy11bPtrTh->siRotorTableCounter = siRotorTableCounter;
    HS_Phy11bPtrTh->outbuff_toggle_th1 = outbuff_toggle;
}


